/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Andre Meixner
* @copyright  2017 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#ifndef _MMM_CONVERTINGSTRATEGY_h_
#define _MMM_CONVERTINGSTRATEGY_h_

#include <MMM/MMMCore.h>
#include <MMM/Motion/Motion.h>
#include <MMMSimoxTools/MMMSimoxTools.h>
#include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h>
#include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h>
#include <VirtualRobot/VirtualRobotCommon.h>
#include <Eigen/Core>

#include <nlopt.hpp>

namespace MMM
{

class MMM_IMPORT_EXPORT ConvertingStrategy
{
public:
    virtual void cancel() = 0;

    virtual float getCurrentTimestep() = 0;

    virtual void convert() = 0;

protected:
    ConvertingStrategy(const std::map<float, std::map<std::string, Eigen::Vector3f> > &labeledMarkerData, MotionPtr outputMotion, ModelPtr outputModel, const std::vector<std::string> &joints, const std::map<std::string, std::string> &markerMapping, const std::map<std::string, float> &markerWeights) :
        nloptAlgorithm(nlopt::LN_SBPLX),
        labeledMarkerData(labeledMarkerData),
        outputMotion(outputMotion),
        outputModel(outputModel),
        outputKinematicSensor(new KinematicSensor(joints)),
        outputModelPoseSensor(new ModelPoseSensor()),
        joints(joints),
        frameDimension(6 + joints.size()),
        markerWeights(markerWeights)
    {
        outputRobot = SimoxTools::buildModel(outputModel, false);
        if (!outputRobot) throw Exception::MMMException("Could not build Simox robot for output model!");
        // Improve performance
        outputRobot = SimoxTools::buildReducedModel(outputRobot, joints);
        outputRobot->setUpdateCollisionModel(false);
        outputRobot->setUpdateVisualization(false);
        outputRobot->setupVisualization(false, false);
        outputRobot->setThreadsafe(false);

        for (const auto &mapping : markerMapping) {
            inputMarkersToOutputRobotSensors[mapping.first] = outputRobot->getSensor(mapping.second);
        }

        for (auto joint : joints) {
            jointValuesToSet[joint] = 0.0f;
        }

        // exceptions will be forwarded
        outputMotion->addSensor(outputKinematicSensor);
        outputMotion->addSensor(outputModelPoseSensor);
    }

    static double objectiveFunctionWrapperStatic(const std::vector<double> &configuration, std::vector<double> &grad, void *data) {
        return static_cast<ConvertingStrategy*>(data)->objectiveFunctionWrapper(configuration, grad);
    }

    double objectiveFunctionWrapper(const std::vector<double> &configuration, std::vector<double> &grad) {
        return objectiveFunction(configuration, grad);
    }

    virtual double objectiveFunction(const std::vector<double> &configuration, std::vector<double> &grad) = 0;

    void setOutputModelConfiguration(const std::vector<double> &configuration) {
        Eigen::Vector3f rootPos;
        rootPos[0] = configuration[0]; rootPos[1] = configuration[1]; rootPos[2] = configuration[2];

        Eigen::Vector3f rootRot;
        rootRot[0] = configuration[3]; rootRot[1] = configuration[4]; rootRot[2] = configuration[5];

        Eigen::Matrix4f globalPose = Math::poseRPYToMatrix4f(rootPos, rootRot);
        outputRobot->setGlobalPose(globalPose, false);

        for (unsigned int i = 0; i < joints.size(); ++i) {
            jointValuesToSet[joints[i]] = configuration[6 + i];
        }

        outputRobot->setJointValues(jointValuesToSet);
    }

    double calculateMarkerDistancesSquaresSum(const std::map<std::string, Eigen::Vector3f> &labeledMarker) const {
        double sumDistancesSquares = 0.0;

        for (const auto &inputMarkersToOutputRobotSensor : inputMarkersToOutputRobotSensors) {
            const std::string &inputMarker = inputMarkersToOutputRobotSensor.first;
            const VirtualRobot::SensorPtr& outputRobotSensor = inputMarkersToOutputRobotSensor.second;

            const Eigen::Vector3f& posInputMarker = labeledMarker.at(inputMarker);
            if(posInputMarker.norm() < 0.0001f) // marker exactly at zero means that it is not valid
            {
                continue;
            }
            const Eigen::Vector3f& posOutputMarker = outputRobotSensor->getGlobalPose().block(0, 3, 3, 1);

            sumDistancesSquares += (posInputMarker - posOutputMarker).squaredNorm() * markerWeights.at(inputMarker);
        }

        return sumDistancesSquares;
    }

    void calculateMarkerDistancesAverageMaximum(const std::map<std::string, Eigen::Vector3f> &labeledMarker, double &avgDistance, double &maxDistance) const {
        double sumDistances = 0.0;
        maxDistance = 0.0;

        for (const auto &inputMarkersToOutputRobotSensor : inputMarkersToOutputRobotSensors) {
            const std::string& inputMarker = inputMarkersToOutputRobotSensor.first;
            const VirtualRobot::SensorPtr& outputRobotSensor = inputMarkersToOutputRobotSensor.second;

            const Eigen::Vector3f& posInputMarker = labeledMarker.at(inputMarker);
            if(posInputMarker.norm() < 0.0001f) // marker exactly at zero means that it is not valid
            {
                continue;
            }
            const Eigen::Vector3f& posOutputMarker = outputRobotSensor->getGlobalPose().block(0, 3, 3, 1);

            double distance = (posInputMarker - posOutputMarker).norm();
            sumDistances += distance;

            if (distance > maxDistance) {
                maxDistance = distance;
            }
        }

        avgDistance = sumDistances / inputMarkersToOutputRobotSensors.size();
    }

    std::pair<double, double> calculateMaxAndAverageJerk(const std::map<float, std::vector<double>>& configurations)
    {
        double jerkSum = 0.0;
        double maxJerk = 0.0;
        std::unique_ptr<Eigen::VectorXd> previousJointPos, previousJointVel, previousJointAcc;
        int frameNum = 0;

        float previousTimestep = 0.0f;
        for(auto& pair : configurations)
        {
            auto timestep = pair.first;
            auto tDelta = timestep - previousTimestep;
            auto frameConfiguration = pair.second;
            setOutputModelConfiguration(frameConfiguration);
            Eigen::VectorXd jointPos = Eigen::Map<Eigen::VectorXd>(frameConfiguration.data(), frameConfiguration.size());
            Eigen::VectorXd orientation = jointPos.block<3,1>(3,0);
            Eigen::VectorXd jointPosWithoutOrientation(jointPos.rows() - 3);
            jointPosWithoutOrientation << jointPos.head(3), jointPos.tail(jointPos.rows()-6); // remove orientation since it cannot be derived like in the following
            jointPosWithoutOrientation.head(3) /= 100; // make position smaller to fit to radian for optimization
            if(previousJointPos)
            {
                /*if(previousJointPos->rows() != (int)frameConfiguration.size())
                {
                    MMM_ERROR << "Different joint vector sizes!" << std::endl;
                    throw std::runtime_error("Different joint vector sizes!");
                }*/

                Eigen::VectorXd jointVel = (jointPosWithoutOrientation - *previousJointPos)/tDelta;
                if(previousJointVel)
                {
                    Eigen::VectorXd jointAcc = (jointVel - *previousJointVel)/tDelta;
                    if(previousJointAcc)
                    {
                        Eigen::VectorXd jointJerk = (jointAcc-*previousJointAcc)/tDelta;
                        if(std::abs(jointJerk.maxCoeff()) > maxJerk)
                        {
                            maxJerk = std::abs(jointJerk.maxCoeff());
                        }
                        jerkSum += jointJerk.cwiseAbs().mean();
                    }
                    else
                    {
                        previousJointAcc.reset(new Eigen::VectorXd());
                    }
                    *previousJointAcc = jointAcc;
                }
                else
                {
                    previousJointVel.reset(new Eigen::VectorXd());
                }
                *previousJointVel = jointVel;
            }
            else
            {
                previousJointPos.reset(new Eigen::VectorXd());
            }
            previousTimestep = timestep;
            *previousJointPos = jointPosWithoutOrientation;
            frameNum++;
        }
        return std::make_pair(maxJerk, jerkSum/frameNum);
    }

    nlopt::algorithm nloptAlgorithm;

    std::map<float, std::map<std::string, Eigen::Vector3f> > labeledMarkerData;
    MotionPtr outputMotion;
    ModelPtr outputModel;
    KinematicSensorPtr outputKinematicSensor;
    ModelPoseSensorPtr outputModelPoseSensor;
    VirtualRobot::RobotPtr outputRobot;
    std::vector<std::string> joints;
    const unsigned int frameDimension;

    // The following members are not strictly necessary but used for performance optimizations
    std::map<std::string, VirtualRobot::SensorPtr> inputMarkersToOutputRobotSensors;
    std::map<std::string, float> jointValuesToSet;
    std::map<std::string, float> markerWeights;

};

typedef boost::shared_ptr<ConvertingStrategy> ConvertingStrategyPtr;

}

#endif
